/*
	Copyright (c) 2010 <copyright holders>
	This source code is licensed under The MIT License,
	see the file LICENSE for details.
	*/

#include "PathFinder.hpp"

#include <cmath>

namespace spelprog2
{
namespace pathfind
{

//Node
Node::Node(int i, sim::Int2d pos, TerrainInfo &ter):
    index_(i),
    position_(pos),
    terrain_(ter)
{
}
Node::Node():
    index_(0)
{
}
int Node::getIndex(){return index_;}
void Node::setIndex(int i){index_ = i;}
sim::Int2d Node::getPosition(){return position_;}
void Node::setPosition(sim::Int2d pos){position_ = pos;}
const std::vector<Node*> &Node::getBranches(){return branches_;}
const TerrainInfo &Node::getTerrainInfo(){return terrain_;}
void Node::setTerrainInfo(TerrainInfo &ter){terrain_ = ter;}
void Node::addBranch(Node *n){branches_.push_back(n);}

//NodeGrid
NodeGrid::NodeGrid(LandscapeSpatials *landscape, int n_width, int n_height, int steepWall)
{
    sim::Int2d extent = landscape->getExtents();
    w_step_ = float(extent.x)/float(n_width);
    h_step_ = float(extent.y)/float(n_height);
    width_ = n_width;
    height_ = n_height;
    
    static int nodeShift[16] = {-1, -1,
                                 0, -1,
                                 1, -1,
                                 1,  0,
                                 1,  1,
                                 0,  1,
                                -1,  1,
                                -1,  0};
    
    nodeList_ = new Node[width_ * height_];
    
    // Read landscape data into nodes and set indices.
    sim::Int2d pos;
    for(int i = 0; i < height_; i++)
    {
        for(int e = 0; e < width_; e++)
        {
            nodeList_[e + width_ * i].setIndex(e + width_ * i);
            pos = sim::Int2d(sim::Int(e*w_step_) + sim::Int(w_step_ * 0.5 + 0.5), sim::Int(i*h_step_) + sim::Int(h_step_ * 0.5 + 0.5));
            nodeList_[e + width_ * i].position_ = pos;
            nodeList_[e + width_ * i].terrain_.height = landscape->getAltitude(pos);
        }
    }
    
    // Create node connections
    for(int i = 0; i < height_; i++)
    {
        for(int e = 0; e < width_; e++)
        {
            for(int g = 0; g < 8; g++)
            {
                if(e + nodeShift[g*2] > 0 && e + nodeShift[g*2] < height_ &&
                   i + nodeShift[g*2 + 1] > 0 && i + nodeShift[g*2 + 1] < width_)
                {
                    if(abs(nodeList_[e + width_ * i].terrain_.height - 
                        nodeList_[e + nodeShift[g*2] + width_ * (i + nodeShift[g*2 + 1])].terrain_.height) < steepWall)
                    {
                        nodeList_[e + width_ * i].addBranch(&nodeList_[e + nodeShift[g*2] + width_ * (i + nodeShift[g*2 + 1])]);
                    }
                }
            }
        }
    }
}
NodeGrid::~NodeGrid()
{
    delete [] nodeList_;
}

Node *NodeGrid::getNodeAtPosition(sim::Int2d position) const
{
    int x = int(float(position.x) / w_step_),
        y = int(float(position.y) / h_step_);
    
    return &nodeList_[x + y*width_];
}
int NodeGrid::getNumNodes() const
{
    return width_*height_;
}

//SearchNode
SearchNode::SearchNode(Node *n, SearchNode *p, int h):
    path_(0),
    value_(0),
    node_(n),
    parent_(p),
    heuristic_(h),
    state_(NO_STATE)
{
    evaluate();
}

void SearchNode::evaluate()
{
    if(!parent_) return;
    
    path_ = parent_->getPathLength();
    sim::Int2d p_pos = parent_->node_->getPosition(),
               pos = node_->getPosition();

	sim::Int dist = (p_pos - pos).length();
	sim::Int heightDiff = node_->getTerrainInfo().height - parent_->node_->getTerrainInfo().height;
    value_ = dist + heightDiff * sim::Int(2);
}

int SearchNode::getPathLength()
{return path_ + value_;}

const std::vector<Node*> &SearchNode::getNodeBranches()
{
    return node_->getBranches();
}

int SearchNode::getValue()
{return value_;}

int SearchNode::getWeight()
{return getPathLength() + heuristic_;}

void SearchNode::tracePath(LandscapeSpatials::PointList & path)
{
    path.clear();
    SearchNode *current = this;
    while(current)
    {
        path.push_back(current->node_->getPosition());
        current = current->parent_;
    }
}

//SearchNodeList
SearchNodeList::SearchNodeList(int size)
{
    nodeList_ = new SearchNode*[size];
    listSize_ = size;
    for(int i = 0; i < listSize_; i++)
    {
        nodeList_[i] = 0;
    }
}
SearchNodeList::~SearchNodeList()
{
    clear();
    delete [] nodeList_;
}

SearchNode *SearchNodeList::pushOpenNode(Node *n, SearchNode *p, int h)
{
    SearchNode *sn = getSearchNode(n, p, h);
    if(sn) pushOpenNode(sn);
    return sn;
}
void SearchNodeList::pushOpenNode(SearchNode *sn)
{
    if(sn->state_ == SearchNode::NO_STATE)
    {
        sn->state_ = SearchNode::OPEN;
        openList_.push_back(sn);
    }
}

SearchNode *SearchNodeList::popOpenNode()
{
    if(openList_.size() <= 0) return 0;
    int element = 0;
    for(size_t i = 0; i < openList_.size(); i++)
    {
        if(openList_[i]->getWeight() < openList_[element]->getWeight())
            element = i;
    }
    
    SearchNode *rn = openList_[element];
    openList_.erase(openList_.begin() + element);
    return rn;
}

SearchNode *SearchNodeList::getSearchNode(Node *n, SearchNode *p, int h)
{
    int index = n->getIndex();
    if(index < listSize_)
    {
        if(!nodeList_[index]) nodeList_[index] = new SearchNode(n, p, h); // Niel Patrick Harris
        if(nodeList_[index]->state_ == SearchNode::NO_STATE) nodeList_[index]->parent_ = p;
        return nodeList_[index];
    }
    return 0;
}
void SearchNodeList::closeNode(SearchNode *sn)
{
    sn->state_ = SearchNode::CLOSED;
}

void SearchNodeList::clear()
{
    for(int i = 0; i < listSize_; i++)
    {
        if(nodeList_[i])
        {
            delete nodeList_[i];
            nodeList_[i] = 0;
        }
    }
}

}; // PathFinder
}; // spelprog2